using System;
using System.Drawing;
using System.Drawing.Imaging;
using System.Runtime.InteropServices;
using System.Threading.Tasks;

namespace MPSizectorDotNet
{
	public class Utils
	{
		public static MP3DFrameManaged RefPlanRectify(MP3DFrameManaged Frame3D, MP3DFrameManaged RefPlan)
		{
			uint num = Frame3D.SensorHeight * Frame3D.SensorWidth;
			MP3DFrameManaged rtv = new MP3DFrameManaged(Frame3D.SensorWidth, Frame3D.SensorHeight);
			rtv.SensorWidth = Frame3D.SensorWidth;
			rtv.SensorHeight = Frame3D.SensorHeight;
			rtv.SensorResolution = Frame3D.SensorResolution;
			rtv.XMax = Frame3D.XMax;
			rtv.YMax = Frame3D.YMax;
			rtv.ZMax = Frame3D.ZMax;
			Parallel.For(0L, num, delegate(long Index)
			{
				rtv.Data[Index] = Frame3D.Data[Index];
				if (Frame3D.Data[Index].Mask == 0 && RefPlan.Data[Index].Mask == 0)
				{
					rtv.Data[Index].Z = Frame3D.Data[Index].Z - RefPlan.Data[Index].Z;
				}
				else
				{
					rtv.Data[Index].Mask = 15;
				}
			});
			return rtv;
		}

		public static void DrawString(Graphics G, string Content, Font StrFont, Color StrColor, Color BackColor, int X, int Y)
		{
			Brush brush = new SolidBrush(BackColor);
			Brush brush2 = new SolidBrush(StrColor);
			G.DrawString(Content, StrFont, brush, new PointF((float)(X - 1), (float)Y));
			G.DrawString(Content, StrFont, brush, new PointF((float)(X + 1), (float)Y));
			G.DrawString(Content, StrFont, brush, new PointF((float)X, (float)(Y - 1)));
			G.DrawString(Content, StrFont, brush, new PointF((float)X, (float)(Y + 1)));
			G.DrawString(Content, StrFont, brush2, new PointF((float)X, (float)Y));
		}

		public unsafe static float MeasureZ(MP3DFrameManaged Frame3D, uint TopLeftX, uint TopLeftY, uint BottomRightX, uint BottomRightY, float DropRatio)
		{
            float result;
            if (Frame3D == null || BottomRightX < 0 || BottomRightX >= Frame3D.SensorWidth || BottomRightY < 0 || BottomRightY >= Frame3D.SensorHeight || TopLeftX < 0 || TopLeftX >= Frame3D.SensorWidth || TopLeftY < 0 || TopLeftY >= Frame3D.SensorHeight || TopLeftX >= BottomRightX || TopLeftY >= BottomRightY || DropRatio > 0.49f || DropRatio < 0f)
			{
				return float.NaN;
			}
            else
            {
                int num = 0;
                fixed (MP3DDataStruct* data = Frame3D.Data)
                {
                    for (uint num2 = TopLeftX; num2 <= BottomRightX; num2 += 1u)
                    {
                        for (uint num3 = TopLeftY; num3 <= BottomRightY; num3 += 1u)
                        {
                            if (data[(ulong)(num2 + Frame3D.SensorWidth * num3) * (ulong)((long)sizeof(MP3DDataStruct)) / (ulong)sizeof(MP3DDataStruct)].Mask == 0)
                            {
                                num++;
                            }
                        }
                    }
                    if (num == 0)
                    {
                        result = float.NaN;
                    }
                    else
                    {
                        float[] array = new float[num];
                        uint num4 = 0u;
                        for (uint num2 = TopLeftX; num2 <= BottomRightX; num2 += 1u)
                        {
                            for (uint num3 = TopLeftY; num3 <= BottomRightY; num3 += 1u)
                            {
                                uint num5 = num2 + Frame3D.SensorWidth * num3;
                                if (data[(ulong)num5 * (ulong)((long)sizeof(MP3DDataStruct)) / (ulong)sizeof(MP3DDataStruct)].Mask == 0)
                                {
                                    array[(int)((UIntPtr)(num4++))] = data[(ulong)num5 * (ulong)((long)sizeof(MP3DDataStruct)) / (ulong)sizeof(MP3DDataStruct)].Z;
                                }
                            }
                        }
                        Array.Sort<float>(array);
                        int num6 = (int)Math.Round((double)((float)(num - 1) * (1f - DropRatio)));
                        int num7 = (int)Math.Round((double)((float)(num - 1) * DropRatio));
                        if (num6 == num7)
                        {
                            result = array[num6];
                        }
                        else
                        {
                            float num8 = 0f;
                            for (int i = num7; i <= num6; i++)
                            {
                                num8 += array[i];
                            }
                            result = num8 / (float)(num6 - num7 + 1);
                        }
                    }
                }
            }
            return result;
        }

		public static MP3DFrameManaged MultiExposureFusion(int FrameNum, MP3DFrameManaged[] Input, int Image2DSourceIndex, bool AverageAllValidPoints)
		{
			uint sensorResolution = Input[0].SensorResolution;
			MP3DFrameManaged Result = new MP3DFrameManaged(Input[0].SensorWidth, Input[0].SensorHeight);
			Result.SensorResolution = sensorResolution;
			Result.XMax = Input[0].XMax;
			Result.YMax = Input[0].YMax;
			Result.ZMax = Input[0].ZMax;
			if (AverageAllValidPoints)
			{
				Parallel.For(0L, sensorResolution, delegate(long Index)
				{
					float num2 = 0f;
					float num3 = 0f;
					float num4 = 0f;
					int num5 = 0;
					for (int j = 0; j < FrameNum; j++)
					{
						if (Input[j].Data[Index].Mask == 0)
						{
							num2 += Input[j].Data[Index].X;
							num3 += Input[j].Data[Index].Y;
							num4 += Input[j].Data[Index].Z;
							num5++;
						}
					}
					if (num5 == 1)
					{
						Result.Data[Index].Mask = 0;
						Result.Data[Index].Gray = Input[Image2DSourceIndex].Data[Index].Gray;
						Result.Data[Index].X = num2;
						Result.Data[Index].Y = num3;
						Result.Data[Index].Z = num4;
					}
					else if (num5 > 1)
					{
						Result.Data[Index].Mask = 0;
						Result.Data[Index].Gray = Input[Image2DSourceIndex].Data[Index].Gray;
						Result.Data[Index].X = num2 / (float)num5;
						Result.Data[Index].Y = num3 / (float)num5;
						Result.Data[Index].Z = num4 / (float)num5;
					}
					else
					{
						Result.Data[Index] = Input[Image2DSourceIndex].Data[Index];
					}
				});
			}
			else
			{
				Parallel.For(0L, sensorResolution, delegate(long Index)
				{
					int num = 0;
					byte b = 0;
					for (int i = 0; i < FrameNum; i++)
					{
						if (Input[i].Data[Index].Mask == 0 && Input[i].Data[Index].Gray >= b)
						{
							num = i;
							b = Input[i].Data[Index].Gray;
						}
					}
					Result.Data[Index] = Input[num].Data[Index];
					Result.Data[Index].Gray = Input[Image2DSourceIndex].Data[Index].Gray;
				});
			}
			return Result;
		}

		public unsafe static Bitmap ConvertGrayByteArrayToGrayBitmap(byte[] Gray, uint Width, uint Height)
		{
			if (Width == 0 || Height == 0)
			{
				throw new ArgumentOutOfRangeException();
			}
			if (Gray.Length != Width * Height)
			{
				throw new ArgumentOutOfRangeException();
			}
			uint count = Width * Height;
			byte[] array = new byte[Width * Height];
			byte[] array2 = new byte[Width * Height];
			byte[] array3 = new byte[Width * Height];
			fixed (byte* value2 = Gray)
			{
				byte[] array4 = array;
				fixed (byte* value = array4)
				{
					Utils.CopyMemory((IntPtr)(void*)value, (IntPtr)(void*)value2, count);
				}
				array4 = array2;
				fixed (byte* value3 = array4)
				{
					Utils.CopyMemory((IntPtr)(void*)value3, (IntPtr)(void*)value2, count);
				}
				array4 = array3;
				fixed (byte* value4 = array4)
				{
					Utils.CopyMemory((IntPtr)(void*)value4, (IntPtr)(void*)value2, count);
				}
			}
			return Utils.ConvertRGBByteArrayToRGBBitmap(array, array2, array3, Width, Height);
		}

		public static Bitmap ConvertRGBByteArrayToRGBBitmap(byte[] Red, byte[] Green, byte[] Blue, uint Width, uint Height)
		{
			if (Width == 0 || Height == 0)
			{
				throw new ArgumentOutOfRangeException();
			}
			if (Red.Length != Width * Height || Green.Length != Width * Height || Blue.Length != Width * Height)
			{
				throw new ArgumentOutOfRangeException();
			}
			Bitmap bitmap = new Bitmap((int)Width, (int)Height, PixelFormat.Format24bppRgb);
			BitmapData bitmapData = bitmap.LockBits(new Rectangle(0, 0, (int)Width, (int)Height), ImageLockMode.WriteOnly, PixelFormat.Format24bppRgb);
			bitmapData.Stride = (int)(Width * 3);
			byte[] ByteArray = new byte[Width * Height * 3];
			Parallel.For(0L, Width * Height, delegate(long i)
			{
				int num = (int)i * 3;
				ByteArray[num] = Blue[i];
				ByteArray[num + 1] = Green[i];
				ByteArray[num + 2] = Red[i];
			});
			Marshal.Copy(ByteArray, 0, bitmapData.Scan0, (int)(Width * Height * 3));
			bitmap.UnlockBits(bitmapData);
			return bitmap;
		}

		public unsafe Bitmap ConvertRGBToGrayBitmap(Bitmap InputBitmap)
		{
			if (InputBitmap == null)
			{
				return null;
			}
			Bitmap bitmap = new Bitmap(InputBitmap.Width, InputBitmap.Height, PixelFormat.Format8bppIndexed);
			BitmapData bitmapData = InputBitmap.LockBits(new Rectangle(0, 0, InputBitmap.Width, InputBitmap.Height), ImageLockMode.WriteOnly, PixelFormat.Format24bppRgb);
			bitmapData.Stride = bitmapData.Width * 3;
			BitmapData bitmapData2 = bitmap.LockBits(new Rectangle(0, 0, InputBitmap.Width, InputBitmap.Height), ImageLockMode.WriteOnly, PixelFormat.Format8bppIndexed);
			byte[] array = new byte[bitmapData.Width * bitmapData.Height * 3];
			byte[] array2 = new byte[bitmapData.Width * bitmapData.Height];
			Marshal.Copy(bitmapData.Scan0, array, 0, bitmapData.Width * bitmapData.Height * 3);
			byte[] array3 = array;
			fixed (byte* ptr = array3)
			{
				array3 = array2;
				fixed (byte* ptr3 = array3)
				{
					byte* ptr2 = ptr;
					byte* ptr4 = ptr3;
					for (int i = 0; i < bitmapData.Width * bitmapData.Height; i++)
					{
						*ptr4 = *ptr2;
						ptr2 += 3;
						ptr4++;
					}
				}
			}
			ColorPalette palette = bitmap.Palette;
			for (int j = 0; j < 256; j++)
			{
				palette.Entries[j] = Color.FromArgb(j, j, j);
			}
			bitmap.Palette = palette;
			Marshal.Copy(array2, 0, bitmapData2.Scan0, bitmapData.Width * bitmapData.Height);
			InputBitmap.UnlockBits(bitmapData);
			bitmap.UnlockBits(bitmapData2);
			return bitmap;
		}

		[DllImport("kernel32.dll")]
		public static extern void CopyMemory(IntPtr dest, IntPtr src, uint count);
	}
}
